package com.wintrisstech.iaroc.sunspot;

import java.io.IOException;

import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.io.IIOPin;
import com.sun.spot.util.BootloaderListener;
import com.sun.spot.util.Utils;
import com.sun.squawk.util.UnexpectedException;
import com.wintrisstech.iaroc.irobot.IRobotCreateConstants;
import com.wintrisstech.iaroc.irobot.IRobotCreateSerialConnection;

public class SunSpotIRobotCreateSerialConnection extends IRobotCreateSerialConnection {
    public EDemoBoard demoBoard;
    protected IIOPin baudRateSelectPin;
    
    public SunSpotIRobotCreateSerialConnection() {
        this(EDemoBoard.getInstance(), EDemoBoard.getInstance().getIOPins()[EDemoBoard.D2]);
    }
    
    public SunSpotIRobotCreateSerialConnection(EDemoBoard demoBoard, IIOPin baudRateSelectPin) {
        this.demoBoard = demoBoard;
        this.baudRateSelectPin = baudRateSelectPin;
        new BootloaderListener().start();
        demoBoard.initUART(EDemoBoard.SERIAL_SPEED_19200, false);
        if (baudRateSelectPin != null) {
            if (IRobotCreateConstants.DEBUG_SERIAL) {
                System.out.println("Triggering baud rate select pin");
            }
            baudRateSelectPin.setAsOutput(true);
            baudRateSelectPin.setHigh();
/* This is supposed to wake/turn on the Create but it does not seem to
            baudRateSelectPin.setLow();
            Utils.sleep(100);
            baudRateSelectPin.setHigh();
            Utils.sleep(2000);
*/
            for (int i = 0; i < 3; i++) {
                baudRateSelectPin.setLow();
                Utils.sleep(250);
                baudRateSelectPin.setHigh();
                Utils.sleep(250);
            }
            try {
                for (int i=0; i < IRobotCreateConstants.MAX_COMMAND_SIZE; i++) {
                    sendByte(IRobotCreateConstants.COMMAND_START);
                }
                sendByte(IRobotCreateConstants.COMMAND_RESET);
                sleep(4000);
                for (int i = 0; i < 3; i++) {
                    baudRateSelectPin.setLow();
                    Utils.sleep(250);
                    baudRateSelectPin.setHigh();
                    Utils.sleep(250);
                }
                flushRead();
                sendByte(IRobotCreateConstants.COMMAND_START);
                while (true) {
                    sendByte(IRobotCreateConstants.COMMAND_SENSORS);
                    sendByte(IRobotCreateConstants.SENSORS_OI_MODE);
                    int mode = readUnsignedByte();
                    if (mode == IRobotCreateConstants.OI_MODE_PASSIVE) {
                        break;
                    }
                }
            } catch (IOException e) {
                throw new UnexpectedException(e);
            }
        }
    }

    public void flushRead() {
        // Empty any data that might be in UART buffer
        int sleepCount = 4;
        while (sleepCount > 0) {
            try {
                demoBoard.receiveUART();
            } catch (IOException e) {
                Utils.sleep(40);
                sleepCount--;
            }
        }
    }

    public int readSignedByte() throws IOException {
        while (true) {
            try {
                int result = demoBoard.receiveUART();
                if (IRobotCreateConstants.DEBUG_SERIAL) {
                    System.out.print("read signed: ");
                    System.out.println(result);
                }
                return result;
            } catch (IOException e) {
                Utils.sleep(20);
            }
        }
    }

    public int readUnsignedByte() throws IOException {
        if (IRobotCreateConstants.DEBUG_SERIAL) {
            System.out.println("read unsigned: ");
        }
        while (true) {
            try {
                int result = demoBoard.receiveUART() & 0xFF;
                if (IRobotCreateConstants.DEBUG_SERIAL) {
                    System.out.print("   ");
                    System.out.println(result);
                }
                return result;
            } catch (IOException e) {
                Utils.sleep(20);
            }
        }
    }

    public int readUnsignedBytes(int[] buffer, int start, int length) throws IOException {
        if (IRobotCreateConstants.DEBUG_SERIAL) {
            System.out.print("read unsigned length: ");
            System.out.println(length);
        }
        for (int i=0; i < length; i++) {
            if (IRobotCreateConstants.DEBUG_SERIAL) {
                System.out.print("   read[");
                System.out.print(i);
                System.out.print("]:");
            }
            while (true) {
                try {
                    buffer[start + i] = demoBoard.receiveUART() & 0xFF;
                    if (IRobotCreateConstants.DEBUG_SERIAL) {
                        System.out.print("   ");
                        System.out.println(buffer[start]);
                    }
                    break;
                } catch (IOException e) {
                    Utils.sleep(20);
                }
            }
        }
        return length;
    }

    public void sendBytePrim(int b) throws IOException {
        if (IRobotCreateConstants.DEBUG_SERIAL) {
            System.out.print("sending: ");
            System.out.println(b);
        }
        demoBoard.sendUART((byte) b);
    }

    public void sendBytesPrim(int[] bytes, int start, int length) throws IOException {
        if (IRobotCreateConstants.DEBUG_SERIAL) {
            System.out.print("sending length: ");
            System.out.println(length);
        }
        for (int i=0; i < length; i++) {
            if (start % 8 == 0) {
                Utils.sleep(20);
            }
            if (IRobotCreateConstants.DEBUG_SERIAL) {
                System.out.print("   sending[");
                System.out.print(i);
                System.out.print("]: ");
                System.out.println(bytes[start + i]);
            }
            demoBoard.sendUART((byte) bytes[start + i]);
        }
    }
    
}
